#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "backinstance.h"
#include "Package/oxfordlasersensordll_global.h"
#include "Package/simpletcpsocketdll_global.h"
#include "Package/SimpleRoboticMath/ConsoleMain/simpleroboticmath.h"

void backinstance::onSeamFindingStart2EndProcess_Mannual(){
    if(this->pLaserSensorData->bRecord){
        int i;
        int j;
        double Point2Camera44[4][4];
        double Tool2PCS44[4][4];
        double tempMatrix[4][4];
        double tempResultMatrix[4][4];

        double tempPosXYZ[3];
        double tempEulerZYX[3];

        ST_LaserSeamDataByPCS tempLaserSeamDataByPCS;

        for(i = 0; i < 4; i++){
            for(j = 0; j < 4; j++){
                Point2Camera44[i][j] = 0;
                Tool2PCS44[i][j] = 0;
                tempMatrix[i][j] = 0;
                tempResultMatrix[i][j] = 0;
            }
        }
        Point2Camera44[0][0] = 1; Point2Camera44[0][3] = 0;
        Point2Camera44[1][1] = 1; Point2Camera44[1][3] = this->pLaserSensorData->y;
        Point2Camera44[2][2] = 1; Point2Camera44[2][3] = this->pLaserSensorData->z;
        Point2Camera44[3][3] = 1;

        Tool2PCS44[0][0] = 1;
        Tool2PCS44[1][1] = 1;
        Tool2PCS44[2][2] = 1;
        Tool2PCS44[3][3] = 1;

        tempPosXYZ[0] = this->pRobotDAQData->KRLRobotPos[0];
        tempPosXYZ[1] = this->pRobotDAQData->KRLRobotPos[1];
        tempPosXYZ[2] = this->pRobotDAQData->KRLRobotPos[2];

        tempEulerZYX[0] = this->pRobotDAQData->KRLRobotPos[3];
        tempEulerZYX[1] = this->pRobotDAQData->KRLRobotPos[4];
        tempEulerZYX[2] = this->pRobotDAQData->KRLRobotPos[5];

        EulerZYX2d4Matrix(tempPosXYZ,tempEulerZYX,Tool2PCS44);
        d4MatrixMultiply(Tool2PCS44,this->pHand2EyesMatrix->mMatrix,tempMatrix);
        d4MatrixMultiply(tempMatrix,Point2Camera44,tempResultMatrix);

        d4Matrix2EulerZYX(tempResultMatrix,tempLaserSeamDataByPCS.POSXYZ,tempLaserSeamDataByPCS.EULERZYX);

        tempLaserSeamDataByPCS.EULERZYX[0] = this->pRobotDAQData->KRLRobotPos[3];
        tempLaserSeamDataByPCS.EULERZYX[1] = this->pRobotDAQData->KRLRobotPos[4];
        tempLaserSeamDataByPCS.EULERZYX[2] = this->pRobotDAQData->KRLRobotPos[5];

        tempLaserSeamDataByPCS.bValue = this->pLaserSensorData->bValue;

        this->vecLaserSeamDataByPCS.push_back(tempLaserSeamDataByPCS);

        this->pLaserSensorData->bRecording = true;
    }
    else{
        if(this->pLaserSensorData->bRecording){
            this->pLaserSensorData->bRecord = false;
            this->pLaserSensorData->bRecording = false;
        }
    }
}

void backinstance::SeamFindingStart2EndProcessMannualCal(){
    int i = 0;
    int size_vecLaserSeamDataByPCS =  this->vecLaserSeamDataByPCS.size();
    this->pSeamFindingData->StartPointDone = false;
    this->pSeamFindingData->EndPointDone = false;

    for(i = 0; i< size_vecLaserSeamDataByPCS; i++){
        if(this->pSeamFindingData->StartPointDone == false){
            if(this->vecLaserSeamDataByPCS[i].bValue == false && this->vecLaserSeamDataByPCS[i+20].bValue == false){
                this->pSeamFindingData->StartPos[0] = this->vecLaserSeamDataByPCS[i].POSXYZ[0];
                this->pSeamFindingData->StartPos[1] = this->vecLaserSeamDataByPCS[i].POSXYZ[1];
                this->pSeamFindingData->StartPos[2] = this->vecLaserSeamDataByPCS[i].POSXYZ[2];
                this->pSeamFindingData->StartPos[3] = this->vecLaserSeamDataByPCS[i].EULERZYX[0];
                this->pSeamFindingData->StartPos[4] = this->vecLaserSeamDataByPCS[i].EULERZYX[1];
                this->pSeamFindingData->StartPos[5] = this->vecLaserSeamDataByPCS[i].EULERZYX[2];
                this->pSeamFindingData->StartPointDone = true;
                i = i + 20;
            }
        }
        else if(this->pSeamFindingData->EndPointDone == false){
            if(this->vecLaserSeamDataByPCS[i].bValue == true && this->vecLaserSeamDataByPCS[i+5].bValue == true){
                this->pSeamFindingData->EndPos[0] = this->vecLaserSeamDataByPCS[i].POSXYZ[0];
                this->pSeamFindingData->EndPos[1] = this->vecLaserSeamDataByPCS[i].POSXYZ[1];
                this->pSeamFindingData->EndPos[2] = this->vecLaserSeamDataByPCS[i].POSXYZ[2];
                this->pSeamFindingData->EndPos[3] = this->vecLaserSeamDataByPCS[i].EULERZYX[0];
                this->pSeamFindingData->EndPos[4] = this->vecLaserSeamDataByPCS[i].EULERZYX[1];
                this->pSeamFindingData->EndPos[5] = this->vecLaserSeamDataByPCS[i].EULERZYX[2];
                this->pSeamFindingData->EndPointDone = true;
            }
        }
    }
}

/*************************************************************************************************************************/
void backinstance::onSeamFindingStart2EndProcess_Auto(){
    if(this->WorkType == ENUM_WorkType::Auto){
        if(this->WorkModel == ENUM_WorkModel::Calibration){
            switch (this->pSeamFindingData->frRobotSeamScan) {
            case 31:
                this->pButtonStatus->PointRecord = true;
                this->pLaserSensorData->bRecord = true;
                this->pButtonStatus->SeamPosCal = false;
                this->pSeamFindingData->toRobotMove = 0;
                this->pSeamFindingData->sendCycle = 0;
                this->onSeamFindingStart2EndProcess_Mannual();
                break;
            case 32:
                this->pButtonStatus->PointRecord = false;
                this->pLaserSensorData->bRecord = false;
                this->pButtonStatus->SeamPosCal = true;
                this->onSeamFindingStart2EndProcess_Mannual();
                this->SeamFindingStart2EndProcessMannualCal();
                if(this->pSeamFindingData->sendCycle == 5){
                    this->pSeamFindingData->toRobotMove = 1;
                }
                else{
                    this->pSeamFindingData->sendCycle = this->pSeamFindingData->sendCycle +1;
                }
                break;
            case 33:
                this->pSeamFindingData->toRobotMove = 2;
                break;
            default:
                break;
            }
        }
    }
}
/*************************************************************************************************************************/

void backinstance::readAutoSeamStartPointResualt(double (&value)[6]){
    value[0] = this->pSeamFindingData->StartPos[0];
    value[1] = this->pSeamFindingData->StartPos[1];
    value[2] = this->pSeamFindingData->StartPos[2];
    value[3] = this->pSeamFindingData->StartPos[3];
    value[4] = this->pSeamFindingData->StartPos[4];
    value[5] = this->pSeamFindingData->StartPos[5];
}

void backinstance::readAutoSeamEndPointResualt(double (&value)[6]){
    value[0] = this->pSeamFindingData->EndPos[0];
    value[1] = this->pSeamFindingData->EndPos[1];
    value[2] = this->pSeamFindingData->EndPos[2];
    value[3] = this->pSeamFindingData->EndPos[3];
    value[4] = this->pSeamFindingData->EndPos[4];
    value[5] = this->pSeamFindingData->EndPos[5];
}
